use cortex_m::asm::delay;
use crate::{flash::FLASH, RCC};

pub fn set_sys_clock_to72() {

    RCC.cr.modify(|r, w| unsafe {
        w.bits(r.bits() | 0x01)
    });
    RCC.cfgr.modify(|r, w| unsafe {
        w.bits(r.bits() & 0xF0FF0000)
    });
    RCC.cr.modify(|r, w| unsafe {
        w.bits(r.bits() & 0xFEF6FFFF)
    });
    RCC.cr.modify(|r, w| unsafe {
        w.bits(r.bits() & 0xFFFBFFFF)
    });
    RCC.cfgr.modify(|r, w| unsafe {
        w.bits(r.bits() & 0xFF80FFFF)
    });
    RCC.cir.write(|w| unsafe { w.bits(0x009F0000) });
    

    RCC.cr.modify(|_, w| w.hseon().on());
    let mut count = 0;

    while RCC.cr.read().hserdy().is_not_ready() {
        delay(1);
        count += 1;
        if count >= 0x500 {
            #[cfg(feature = "defmt")]
            defmt::error!("rcc init timeout");
            return;
        }
    }
    FLASH.acr.modify(|_, w| w.prftbe().set_bit());
    FLASH.acr.modify(|_, w| w.latency().ws2());
    
    RCC.cfgr.modify(|_, w| {
        w.hpre().div1()
        .ppre2().div1()
        .ppre1().div2()
        .pllsrc().hse_div_prediv()
        .pllxtpre().div1()
        .pllmul().mul9()
    });
    RCC.cr.modify(|_, w| w.pllon().on());
    while RCC.cr.read().pllrdy().is_not_ready() {}

    RCC.cfgr.modify(|_, w| w.sw().pll());
    while !RCC.cfgr.read().sws().is_pll() {}
}


